function [xr, yr, kr, thetar] = B_Spline_Create(path_original, num_B)
%===根据控制点生成B样条曲线===%
% 编写者：李正
% 编写日期：2021年6月9日
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%% 控制点读取
P = path_original;
P = P';

%% 输入控制点个数n+1，阶数k
n = length(P) - 1; % n为控制顶点个数-1
k = 4;             % 二阶曲率连续只需要4阶（3次）曲线，因此k=4

%% 设置B样条曲线轨迹点个数
% num_B_point = round((n+1)/k); % 对结果四舍五入
num_B_point = 200;
path = zeros(2,num_B_point);  % 为曲线预分配变量空间，第一行为横坐标，第二行为纵坐标，列数为num_B_point

%% 基函数预分配变量空间
Bik_u = zeros(n+1,1); %基函数的数量与控制顶点数量相同，Bik为n+1个元素的列向量

%% 生成准均匀B样条曲线的节点向量
NodeVector = My_NodeVector_quasi_uniform(n , k);

%% 创建参数u 
u = linspace(0 , 1 - 1 / num_B_point , num_B_point);% 参数u为从0到1-1/num_B_point等间距分布的num_B_point个点

%% 循环生成B样条曲线
for i_num = 1 : num_B_point
    for i = 1 : n+1 % 推导为0：n，此处是为了方便子程序编写
        Bik_u(i,1) = My_BaseFunction(i , k , u(i_num) , NodeVector);  % 调用子函数生成基函数
    end
    
    P_u = P * Bik_u; % B样条曲线数学表达式
    path(1,i_num) = P_u(1); % 将轨迹点存入预先分配的变量空间
    path(2,i_num) = P_u(2); % 将轨迹点存入预先分配的变量空间
end

%% 输出纵向坐标、横向坐标
xr=path(1,:);
yr=path(2,:);

%% 输出曲率
% 一阶导数
pos_dot=zeros(1,length(path));    
for i = 1 : length(path) - 1
    pos_dot(i) = (yr(i+1) - yr(i)) / (xr(i+1) - xr(i));%差分格式近似代替求导
end
pos_dot(end) = pos_dot(end-1);                                         %设置最后一个轨迹点的一阶导数

% 计算二阶导数
dpos_dot=zeros(1,length(path));    
for i = 2 : length(path) - 1
    dpos_dot(i) = (yr(i+1)-2*yr(i)+yr(i-1))...
                  / (0.5*(xr(i+1)-xr(i-1)))^2;                         %差分格式近似代替求导
end
dpos_dot(1) = dpos_dot(2);                                             %设置第一个轨迹点的二阶导数
dpos_dot(end) = dpos_dot(end-1);                                       %设置最后一个轨迹点的二阶导数

% 曲率求解
kr=zeros(1,length(path));
for i = 1 : length(path)
    kr(i)=(dpos_dot(i))/(1+pos_dot(i)^2)^1.5;                          %曲率计算公式
end

%% 输出航向角
thetar=zeros(1,length(path));
vector_X=[1,0];%定义绝对坐标系X轴单位基向量,目的是采用点积计算航向角

for i = 1 : length(path) - 1
    vector_vehicle=[(xr(i+1) - xr(i)),(yr(i+1) - yr(i))];%定义车辆速度方向向量
    if yr(i+1) > yr(i)                                                   %一二象限内，采用点积公式求出的角度即为航向角
        thetar(i)=acos((vector_X * vector_vehicle') / ...
                  (sqrt((xr(i+1) - xr(i))^2+(yr(i+1) - yr(i))^2)));
    else                                                                 %三四象限内，求出的角度为0到pi，需要用2*pi减去这个角度
        thetar(i)=2*pi - acos((vector_X * vector_vehicle') / ...
                  (sqrt((xr(i+1) - xr(i))^2+(yr(i+1) - yr(i))^2)));
    end
end
thetar(end)=thetar(end-1);                                               %设置最后一个轨迹点的航向角
end

